program checksimul
use normeanvar
use datahadler
use matrixfuns
use imhoff
use linear_operators
implicit none
integer, parameter:: n=1,T=5000,num=100
integer, parameter:: p=n+(n*(n+1)/2)
double precision,parameter:: si=.999d0
double precision dmut(p,n),dvsig(p,n*n),sigma(n,n),matA(p,p),matB(n+1,p)&
&,matC(n+1,p+n+1),mtest1(n+1,1),vtest1(n+1,n+1),MD(p+n+1)&
&,VD(p+n+1,p+n+1),dmat(n,n),wmat(n+1,n+1),pr,dchiin,b(n),poweru(num,5)& !suplm,kt,skew,kurt2s,kurt1s
&,eta(num),crit,critkt,critk2s,critk1s,critskew,bmult(3)&
&,powerk(num),meank,vark,critk,dnorin,dnordf,errest
integer i,j,k,ifault

eta=linspace(.00001d0,.02d0,num)
bmult=vassign(3,0.75d0)

forall(i=1:n+1,j=1:n+1)
	wmat(i,j)=0.0d0
end forall
forall(i=1:p,j=1:n)
dmut(i,j)=0.0d0
end forall
forall(i=1:p,j=1:n*n)
dvsig(i,j)=0.0d0
end forall

dmut(1,1)=1.0d0
dvsig(2,1)=1.0d0

sigma(1,1)=1.0d0

!sigma=eye(3)
crit=dchiin(.95d0,dble(n+1))
critk2s=dnorin(.975d0)
critk1s=dnorin(.95d0)
critskew=dchiin(.95d0,dble(n))*2.0d0*(n+2)
critkt=mixchi2inv(.95d0,dble(n),dble(n)+1.0d0)

do i=1,num,1
	b(1)=buniv(bmult,eta(i))


call meanvar(matA,matB,MD,VD,b,eta(i),si,sigma,dmut,dvsig,n,p)
matC(:,1:p)=matmul(matB,(.i.matA))
matC(:,p+1:p+n+1)=eye(n+1)

mtest1(:,1)=MD(p+1:p+n+1)*dsqrt(dble(T))
vtest1=(matC.x.VD).xt.matC

wmat(1,1)=2.0d0/(n*(n+2.0d0))
wmat(2,2)=(.5d0/(n+2.0d0))/sigma(1,1)

!Sup-LM
call imhofprob(pr,ifault,mtest1,vtest1,wmat,crit)
poweru(i,1)=1-pr

!! Kuhn-Tucker
call dqdagi(integ,1.0d0,2,0.0d0,1.d-8,pr,errest)
poweru(i,2)=1.0d0-pr

!Skewness
poweru(i,3)=1.0d0-dnordf((dsqrt(critskew)-mtest1(2,1))/dsqrt(vtest1(2,2)))&
&+dnordf((-dsqrt(critskew)-mtest1(2,1))/dsqrt(vtest1(2,2)))

!Kurtosis 2s
meank=mtest1(1,1)*dsqrt(wmat(1,1))
vark=vtest1(1,1)*wmat(1,1)
poweru(i,4)=1.0d0-dnordf((critk2s-meank)/dsqrt(vark))+dnordf((-critk2s-meank)/dsqrt(vark))

!Kurtosis 1s
meank=mtest1(1,1)*dsqrt(wmat(1,1))
vark=vtest1(1,1)*wmat(1,1)
poweru(i,5)=1.0d0-dnordf((critk1s-meank)/dsqrt(vark))


print*, i,b !,ifault,poweru(i,2),poweru(i,5)
end do

call exportvec(eta,"et1.txt",1)
call exportvec(vec2(poweru),"p_aym1u.txt",1)

contains

function integ(x)
double precision x,integ,med,var,medk,vark,c,lmk,prx
double precision, parameter:: pi=3.14159265358979d0
medk=mtest1(1,1)
vark=vtest1(1,1)

med=mtest1(2,1)+(vtest1(2,1)/vark)*(x-medk)
var=vtest1(2,2)-(1.0d0/vark)*(vtest1(2,1)*vtest1(2,1))

if (x>0.0d0) then
	c=(critkt-(x*x*2.0d0/(n*(n+2.0d0))))*2.0d0*(n+2.0d0)
else
	c=critkt*2.0d0*(n+2.0d0)
end if
if (c<0) then
	prx=0.0d0
else
	prx=dnordf((dsqrt(c)-med)/dsqrt(var))-dnordf((-dsqrt(c)-med)/dsqrt(var))
end if


integ=prx*dexp(-.5d0*(x-medk)*(x-medk)/vark)/dsqrt(2.0d0*pi*vark)
end function integ

function mixchi2inv(p,n1,n2)
double precision p, n1,n2,a,b,c,obja,objb,objc,mixchi2inv,dchidf
double precision, parameter:: tol=1.0d-5

a=0.0d0
b=10.0d0
obja=.5d0*dchidf(a,n1)+.5d0*dchidf(a,n2)-p
objb=.5d0*dchidf(b,n1)+.5d0*dchidf(b,n2)-p

do while (obja*objb>0.0d0)
	a=a-1.0d0
	b=b+1.0d0
end do 

do while (dabs(a-b)>tol)
	c=(a+b)/2.0d0
	objc=.5d0*dchidf(c,n1)+.5d0*dchidf(c,n2)-p
	if (objc>0) then
		b=c
	else
		a=c
	end if
end do
mixchi2inv=(a+b)/2.0d0
end function mixchi2inv

function buniv(beta,eta)
double precision beta(:),eta,buniv,normb,c,a

a=2.0d0*eta/(1.0d0-4.0d0*eta)
normb=dot_product(beta,beta)

if (dot_product(beta,beta).gt.0.0d0) then
	c=(-1.0d0+dsqrt(1.0d0+4.0d0*a*normb))/(2.0d0*a*normb)
	buniv=c*beta(1)/(1.0d0+(c-1)*(beta(1)*beta(1)/dot_product(beta,beta)))
else 
	buniv=0.0d0
end if

end function buniv
end program checksimul